#include<iostream>
#include<opencv2/opencv.hpp>
 
using namespace std;
using namespace cv;
 
 
class SAD
{
private:
    int winSize;//卷积核尺寸
    int DSR;//视差搜索范围
public:
    SAD() :winSize(7), DSR(30){}
    SAD(int _winSize, int _DSR) :winSize(_winSize), DSR(_DSR){}
    Mat computerSAD(Mat&L, Mat&R);//计算SAD
};
 
Mat SAD::computerSAD(Mat&L, Mat&R)
{
    int Height = L.rows;
    int Width = L.cols;
    Mat Kernel_L(Size(winSize, winSize), CV_8U, Scalar::all(0));
    //CV_8U:0~255的值，大多数图像/视频的格式，该段设置全0矩阵
    Mat Kernel_R(Size(winSize, winSize), CV_8U, Scalar::all(0));
    Mat Disparity(Height, Width, CV_8U, Scalar(0));
 
 
    for (int i = 0; i < Width - winSize; ++i){
        for (int j = 0; j < Height - winSize; ++j){
            Kernel_L = L(Rect(i, j, winSize, winSize));//L为做图像，Kernel为这个范围内的左图
            Mat MM(1, DSR, CV_32F, Scalar(0));//定义匹配范围
 
            for (int k = 0; k < DSR; ++k){
                int x = i - k;
                if (x >= 0){
                    Kernel_R = R(Rect(x, j, winSize, winSize));
                    Mat Dif;
                    absdiff(Kernel_L, Kernel_R, Dif);
                    Scalar ADD = sum(Dif);
                    float a = ADD[0];
                    MM.at<float>(k) = a;
                }
                Point minLoc;
                minMaxLoc(MM, NULL, NULL, &minLoc, NULL);
 
                int loc = minLoc.x;
                Disparity.at<char>(j, i) = loc * 16;
            }
            double rate = double(i) / (Width);
            cout << "已完成" << setprecision(2) << rate * 100 << "%" << endl;
        }
    }
    return Disparity;
}
 
int main()
{
    Mat left = imread("/home/chyang/Documents/program/slam/chapter_03/src/left.png");
    Mat right = imread("/home/chyang/Documents/program/slam/chapter_03/src/right.png");
    //-------图像显示-----------
    // namedWindow("leftimag");
    // imshow("leftimag", left);
 
    // namedWindow("rightimag");
    // imshow("rightimag", right);
    //--------由SAD求取视差图-----
    Mat Disparity;
 
    SAD mySAD(7, 30);
    Disparity = mySAD.computerSAD(left, right);
    //-------结果显示------
    imwrite("sad.bmp", Disparity);
    //-------收尾------
    return 0;
}